package com.dizner.androidcartest;

import android.os.SystemClock;
import android.support.annotation.FloatRange;
import android.support.annotation.IntRange;

import com.google.android.things.pio.Pwm;

import org.greenrobot.eventbus.EventBus;
import org.greenrobot.eventbus.Subscribe;

import java.io.IOException;
import java.util.Iterator;
import java.util.Map;
import java.util.concurrent.ConcurrentHashMap;

/**
 * 适配至驱动板
 */
public class RaspberryPI3CarDiver implements AutoCloseable, HWSensor.OnHWSensorHasDataListener {

    private static final String TAG = "RaspberryPI3CarDiver";

    private MotorHAT mMotorHAT;

    private MotorHAT.DcMotor mMotor_left;
    private MotorHAT.DcMotor mMotor_right;
    public static final String HW_SENSOR_LEFT = "hw_sensor_left";
    public static final String HW_SENSOR_RIGHT = "hw_sensor_right";

    public boolean enableForward = true;
    private boolean enableLeft = true, enableRight = true, enableBack = true;


    private RaspberryPI3CarDiver(MotorHAT motorHAT, int motor_num_left, int motor_num_right) {
        this.mMotorHAT = motorHAT;
        if (mMotorHAT != null) {
            mMotor_left = mMotorHAT.getMotor(motor_num_left);
            mMotor_right = mMotorHAT.getMotor(motor_num_right);
            try {
                mMotor_left.setSpeed(100);
                mMotor_right.setSpeed(100);
            } catch (IOException e) {
                e.printStackTrace();
            }
            if (!EventBus.getDefault().isRegistered(this)) {
                EventBus.getDefault().register(this);
            }
        } else {
            throw new IllegalArgumentException("驱动板驱动错误");
        }
    }

    static RaspberryPI3CarDiver create(MotorHAT motorHAT, @IntRange(from = 1, to = 4) int motor_num_left, @IntRange(from = 1, to = 4) int motor_num_right) {
        return new RaspberryPI3CarDiver(motorHAT, motor_num_left, motor_num_right);
    }

    void forward(long miles) {

        try {
            if (!enableForward) {
                stop();
                return;
            }
            mMotor_left.commond(MotorHAT.MOTOR_STATE_CCW);
            mMotor_right.commond(MotorHAT.MOTOR_STATE_CW);
        } catch (IOException e) {
            e.printStackTrace();
        }
    }

    public void forwardEnd() {
        RaspberryPI3CarDiver.this.stop();
    }


    void back(long miles) {

        try {
            if (!enableBack) {
                stop();
                return;
            }
            mMotor_left.commond(MotorHAT.MOTOR_STATE_CW);
            mMotor_right.commond(MotorHAT.MOTOR_STATE_CCW);
        } catch (IOException e) {
            e.printStackTrace();
        }

    }


    public void backEnd() {
        stop();
    }


    void right(long miles) {
        try {
            if (!enableRight) {
                stop();
                return;
            }
            mMotor_left.commond(MotorHAT.MOTOR_STATE_CW);
            mMotor_right.commond(MotorHAT.MOTOR_STATE_CW);
        } catch (IOException e) {
            e.printStackTrace();
        }
    }

    public void rightEnd() {
        stop();
    }


    void left(long miles) {
        try {
            if (!enableLeft) {
                stop();
                return;
            }
            mMotor_left.commond(MotorHAT.MOTOR_STATE_CCW);
            mMotor_right.commond(MotorHAT.MOTOR_STATE_CCW);
        } catch (IOException e) {
            e.printStackTrace();
        }
    }


    public void leftEnd() {
        stop();
    }

    void ready() {
        stop();
    }


    void stop() {
        //刹车
        try {
            mMotor_left.commond(MotorHAT.MOTOR_STATE_RELEASE);
            mMotor_right.commond(MotorHAT.MOTOR_STATE_RELEASE);
        } catch (IOException e) {
            e.printStackTrace();
        }
    }

    //刹车
    void stop(long miles) {
        stop();
    }


    void stopEnd() {
        stop();
    }


    private void delayedAndStop(long miles) {
        if (miles <= 0) {
            return;
        }
        try {
            int temp = 0;

            while (true) {
                temp++;
                Thread.sleep(1);
                if (temp > miles) {
                    break;
                }
            }

        } catch (InterruptedException e) {
            e.printStackTrace();
        }
        stop();
    }


    public boolean isEnableForward() {
        return enableForward;
    }

    public void setEnableForward(boolean enableForward) {
        this.enableForward = enableForward;
    }

    public boolean isEnableLeft() {
        return enableLeft;
    }

    public void setEnableLeft(boolean enableLeft) {
        this.enableLeft = enableLeft;
    }

    public boolean isEnableRight() {
        return enableRight;
    }

    public void setEnableRight(boolean enableRight) {
        this.enableRight = enableRight;
    }

    public boolean isEnableBack() {
        return enableBack;
    }

    public void setEnableBack(boolean enableBack) {
        this.enableBack = enableBack;
    }

    /**
     * max 100
     * min 0
     *
     * @param speed
     */
    public void changeSpeed(@FloatRange(from = 1, to = 100) float speed) {
        try {
            speed /= 100;
            if (speed > 0 && speed <= 0.3) {
                speed = 0.3f;
            }
            mMotor_left.setSpeed((int) (150f * speed));
            mMotor_right.setSpeed((int) (150f * speed));
        } catch (IOException e) {
            e.printStackTrace();
        }
    }

    @Subscribe
    public void onEvent(String msg) {
        switch (msg) {
            case "hasZW":
                enableForward = false;
                break;
            case "noZW":
                enableForward = true;
                break;
        }
    }

    @Override
    public void close() throws Exception {
        if (EventBus.getDefault().isRegistered(this)) {
            EventBus.getDefault().unregister(this);
        }
    }

    @Override
    public void onHasData(String sensorName, boolean hasZA) {
        switch (sensorName) {
            case HW_SENSOR_LEFT:
                enableRight = !hasZA;
                break;
            case HW_SENSOR_RIGHT:
                enableLeft = !hasZA;
                break;
        }
    }
}
